#ifndef _RTK_H_
#define _RTK_H_

#include "rads_base.hpp"

#include <iostream>  
#include <sstream>  
#include <fcntl.h>  
#include <termios.h>  
#include <unistd.h>  
#include <cstring>  
#include <vector> 
#include <thread>
#include <cmath>
#include <chrono> 
#include <ctime> 
#include <condition_variable>
// socket tcp for rtcm 
#include <arpa/inet.h>  
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>

#include <boost/bind.hpp>

#include <ros/ros.h>
#include <ros/time.h>
#include <std_msgs/String.h>
#include <std_msgs/ByteMultiArray.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/NavSatStatus.h>
#include <std_msgs/Int16.h>

#include <rads/rads_trackext.h>


const char client_agent[] = "NTRIP NTRIPClient/20181206";
const char base64_code_table[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";

typedef struct
{
	char data[DIFF_DATA_LEN];
	size_t  len;
	int is_ready;
}QxDiffData;

enum DECODE_GPYBM 
{
	DG_HEAD = 0,
	DG_SN = 1,
	DG_UTC = 2,
	DG_LAT = 3,
	DG_LNG = 4,
	DG_ALT = 5,
	DG_YAW = 6,
	DG_PITCH = 7,
	DG_SPD_NORTH = 8,
	DG_SPD_EAST = 9,
	DG_SPD_DOWN = 10,
	DG_SPD_GROUND = 11,
	DG_GAUSS_NX = 12,
	DG_GAUSS_EY = 13,
	DG_STATION_NX = 14,
	DG_STATION_EY = 15,
	DG_FIX_MASTER = 16,
	DG_FIX_SLAVE = 17,
	DG_MASTER_STARS = 18,
	DG_DIFF_DELAY = 19,
	DG_STATION_ID = 20,
	DG_BASE_LENGTH = 21,
	DG_SLAVE_STARS = 22,
	DG_CHECK = 23,
};

typedef struct
{
	std::string head;
	std::string sn;
	std::tm utc;
	std::string lat;
	std::string lng;
	std::string alt;
	std::string yaw;
	std::string pitch;
	double spd_north;
	double spd_east;
	double spd_down;
	double spd_groud;
	double gauss_nx;
	double gauss_ey;
	double station_nx;
	double station_ey;
	int master_stars;
	int slave_stars;
	int fix_master;
	int fix_slave;
	int diff_delay;
	int station_id;
	double base_length;
}NEMA_GPYBM;

//
char index2chr(int index)
{
	return base64_code_table[index];
}

// 用于GPGGA的时间转换(string转tm)
std::tm string2Tm(const std::string& timeStr) {  
	std::tm tm = {}; 
	if (timeStr.length() < 6) {  
		throw std::invalid_argument("Time string must be in HHMMSS format.");  
	}  
	tm.tm_hour = safe_stoi(timeStr.substr(0, 2));
	tm.tm_min = safe_stoi(timeStr.substr(2, 2));
	tm.tm_sec = safe_stoi(timeStr.substr(4, 2));
	// 获取当前系统时间作为年月日的补充
	std::time_t currentTime = std::time(nullptr); 
	std::tm* lt = std::localtime(&currentTime);
	tm.tm_year = 1900 + lt->tm_year;
	tm.tm_mon = 1 + lt->tm_mon;
	tm.tm_mday = lt->tm_mday;
	return tm;  
}

// 当前时间 utc hhmmss.ss的字符串 
std::string curUTCTime() {  
	auto now = std::chrono::system_clock::now();  
	std::time_t now_time_t = std::chrono::system_clock::to_time_t(now);  
	std::tm* now_tm = std::localtime(&now_time_t);
	auto duration_since_epoch = now.time_since_epoch();  
	auto milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(duration_since_epoch).count() % 1000;
	std::ostringstream oss;  
	oss << std::setw(2) << std::setfill('0') << (now_tm->tm_hour)  
		<< std::setw(2) << std::setfill('0') << (now_tm->tm_min)  
		<< std::setw(2) << std::setfill('0') << (now_tm->tm_sec)  
		<< "."  
		<< std::setw(3) << std::setfill('0') << (milliseconds/10);
	return oss.str();  
}  

// UTC打印各个属性信息
void tmPrint(const std::tm utcTime)
{
	std::cout << "UTC Time: ";  
	std::cout << (utcTime.tm_year) << "-";  
	std::cout << (utcTime.tm_mon) << "-";  
	std::cout << utcTime.tm_mday << " ";  
	std::cout << utcTime.tm_hour << ":";  
	std::cout << std::setfill('0') << std::setw(2) << utcTime.tm_min << ":";  
	std::cout << std::setfill('0') << std::setw(2) << utcTime.tm_sec << std::endl;  
}

// 逗号分隔
std::vector<std::string> splitString(const std::string& str) {  
	std::vector<std::string> tokens;  
	std::string token;  
	std::istringstream tokenStream(str);  
	while (std::getline(tokenStream, token, ',')) {  
		tokens.push_back(token);  
	}   
	return tokens;  
} 

// 用于dd转ddmm.mmmm
std::string DD2DM(double decimalDegrees) {  
	bool isNegative = decimalDegrees < 0;  
	decimalDegrees = std::fabs(decimalDegrees);  
	int degrees = static_cast<int>(decimalDegrees);  
	double decimalMinutes = (decimalDegrees - degrees) * 60;  
	int minutesInt = static_cast<int>(decimalMinutes);  
	double minutesFractional = decimalMinutes - minutesInt;  
	std::ostringstream oss,oss2;  
	oss2 << std::fixed << std::setprecision(4) << minutesFractional;  
	oss << (isNegative ? "-" : "")  
		<< std::setw(2) << std::setfill('0') << degrees << std::setw(2) << std::setfill('0') << minutesInt;  
	if (minutesFractional > 0.0) {  
		std::string mf = oss2.str();
		if (mf.length() >= 2) {  
			mf.erase(0, 2);  
		}  
		oss << "." << mf;  
	} else {  
		oss << ".0000"; 
	}  
	return oss.str();  
}

// 自动生成GPGGA数据(+"\r\n")
std::string generateGPGGA(double lat, double lng,int status) {  
	char latDir = lat >=0 ? 'N':'S';
	char lonDir = lng >=0 ? 'E':'W';
	std::string _lat = std::to_string(lat);
	std::string _lng = std::to_string(lng);
	std::string _utc = curUTCTime();
	int fixQuality = status;	// 3D fix  
	std::ostringstream gpggaStr;  
	gpggaStr << "GPGGA,"  
		<< _utc << ","  
		<< _lat << ","  
		<< latDir << ","  
		<< _lng << ","  
		<< lonDir << ","  
		<< fixQuality << ",08,1.0,0.000,M,100.000,M,,"; 
	int checksum = 0;  
	for (char c : gpggaStr.str()) {  
		checksum ^= static_cast<unsigned char>(c);  
	}
	gpggaStr << "*" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') << checksum;  
	return "$" + gpggaStr.str() + "\r\n";  
}

// 串口读取到GPYBM的数据解析成NEMA_GPYBM结构的对象
NEMA_GPYBM nema2gpybm(const std::vector<std::string>& nema)
{
	NEMA_GPYBM gpybm;
	if(GPYBM_LENGTH == nema.size())
	{
		gpybm.head = nema.at(DG_HEAD);
		gpybm.sn = nema.at(DG_SN);
		gpybm.utc = string2Tm(nema.at(DG_UTC));
#if CURRENT_DEBUG_LEVEL >= DEBUG_LEVEL_INFO
		//		tmPrint(gpybm.utc);
#endif
		gpybm.lat = nema.at(DG_LAT);		
		gpybm.lng = nema.at(DG_LNG);		
		gpybm.alt = nema.at(DG_ALT);		
		gpybm.yaw = nema.at(DG_YAW);		
		gpybm.pitch = nema.at(DG_PITCH);
		gpybm.spd_north = safe_stod(nema.at(DG_SPD_NORTH));
		gpybm.spd_east = safe_stod(nema.at(DG_SPD_EAST));
		gpybm.spd_down = safe_stod(nema.at(DG_SPD_DOWN));
		gpybm.spd_groud = safe_stod(nema.at(DG_SPD_GROUND));
		gpybm.gauss_nx = safe_stod(nema.at(DG_GAUSS_NX));
		gpybm.gauss_ey = safe_stod(nema.at(DG_GAUSS_EY));
		gpybm.station_nx = safe_stod(nema.at(DG_STATION_NX));
		gpybm.station_ey = safe_stod(nema.at(DG_STATION_EY));
		gpybm.master_stars = safe_stoi(nema.at(DG_MASTER_STARS));
		gpybm.slave_stars = safe_stoi(nema.at(DG_SLAVE_STARS));
		gpybm.fix_master = safe_stoi(nema.at(DG_FIX_MASTER));
		gpybm.fix_slave = safe_stoi(nema.at(DG_FIX_SLAVE));
		gpybm.diff_delay = safe_stoi(nema.at(DG_DIFF_DELAY));
		gpybm.station_id = safe_stoi(nema.at(DG_STATION_ID));
		gpybm.base_length = safe_stod(nema.at(DG_BASE_LENGTH));
	}
	return gpybm;
}


// base64
int base64_encode(char *src, char *result)
{
	char temp[3] = {0};
	int i = 0, j = 0, count = 0;
	int len = strlen(src);
	if (len == 0)
		return -1;
	if (len % 3 != 0)
	{
		count = 3 - len % 3;
	}
	while (i < len)
	{
		strncpy(temp, src + i, 3);
		result[j + 0] = index2chr((temp[0] & 0xFC) >> 2);
		result[j + 1] = index2chr(((temp[0] & 0x3) << 4) | ((temp[1] & 0xF0) >> 4));
		if (temp[1] == 0)
			break;
		result[j + 2] = index2chr(((temp[1] & 0xF) << 2) | ((temp[2] & 0xC0) >> 6));
		if (temp[2] == 0)
			break;
		result[j + 3] = index2chr(temp[2] & 0x3F);
		i += 3;
		j += 4;
		memset(temp, 0x0, 3);
	}
	while (count)
	{
		result[j + 4 - count] = '=';
		--count;
	}
	return 0;
}

// 千寻授权码
char *authCode()
{
	static char request_data[1024] = {0};
	char userinfo_raw[48] = {0};
	char userinfo[64] = {0};
	char mountpoint[] = QX_MOUNT;
	char user[] = QX_USER;
	char passwd[] = QX_USERPASS;
	sprintf(userinfo_raw, "%s:%s", user, passwd);
	base64_encode(userinfo_raw, userinfo);
	sprintf(request_data,
			"GET /%s HTTP/1.1\r\n"
			"User-Agent: %s\r\n"
			"Accept: */*\r\n"
			"Connection: close\r\n"
			"Authorization: Basic %s\r\n"
			"\r\n",
			mountpoint, client_agent, userinfo);
	return request_data;
}   

class SerialPortRtk {  
	public:  
		SerialPortRtk(const std::string& portName, speed_t baudRate): portName_(portName), baudRate_(baudRate), fd_(-1) {}  
		bool open() {  
			fd_ = ::open(portName_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);  
			if (fd_ == -1) {
				return false;  
			}  
			struct termios options;  
			tcgetattr(fd_, &options);  
			// Set baud rate  
			cfsetispeed(&options, baudRate_);  
			cfsetospeed(&options, baudRate_);  
			// Set 8N1 (8 data bits, no parity, 1 stop bit)  
			options.c_cflag &= ~PARENB; // No parity  
			options.c_cflag &= ~CSTOPB; // 1 stop bit  
			options.c_cflag &= ~CSIZE;  
			options.c_cflag |= CS8;     // 8 data bits  
			// Enable receiver, ignore modem control lines  
			options.c_cflag |= (CLOCAL | CREAD);  
			// Set raw input/output  
			options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);  
			options.c_oflag &= ~OPOST;  
			// Apply settings  
			tcsetattr(fd_, TCSANOW, &options);  
			// Set read timeout to 0 (non-blocking or blocking depending on fcntl below)  
			struct termios tty;  
			tcgetattr(fd_, &tty);  
			tty.c_cc[VMIN]  = 0;  
			tty.c_cc[VTIME] = 1; // 0.5 seconds read timeout  
			tcsetattr(fd_, TCSANOW, &tty);  
			// Set the file descriptor to blocking mode  
			fcntl(fd_, F_SETFL, 0);  
			return true;  
		}  
		void close() {  
			if (fd_ != -1) {  
				::close(fd_);  
				fd_ = -1;  
			}  
		}
		bool writeLine(const char* buffer,size_t length)
		{
			if(-1 == fd_) return false;
			ssize_t bytesWrite = write(fd_,buffer,length);
			if(-1 == bytesWrite) return false;
			if(static_cast<size_t>(bytesWrite) != length)
			{
				DEBUG_ERROR("Partial write to serial port");	
				return false;
			}	
			return true;
		} 
		bool readLine(std::string& line) {  
			char buffer[256];  
			ssize_t numRead;  
			line.clear();  
			while (true) {  
				numRead = read(fd_, buffer, sizeof(buffer) - 1);  
				if (numRead == -1) {  
					DEBUG_WARN("No data read");
					return false;  
				} else if (numRead == 0) {  
					continue;  
				}  
				buffer[numRead] = '\0';  
				line.append(buffer, numRead);  
				if (line.find('\n') != std::string::npos) {  
					line.erase(line.find_last_not_of("\r\n") + 1); // Remove trailing CR/LF  
					return true;  
				}  
			}  
		}  
		bool startsWith(const std::string& str, const std::string& prefix) {  
			return str.compare(0, prefix.length(), prefix) == 0;  
		}
	private:  
		std::string portName_;  
		speed_t baudRate_;  
		int fd_;  
};  


class TCPSocket {  
	public:  
		TCPSocket();  
		~TCPSocket();  
		bool initialize();  
		bool connectToServer(const char* ipAddress, int port);  
		bool sendData(const char* data, size_t length);  
		bool receiveData(char* buffer, size_t bufferSize, size_t& receivedLength);  
		bool sdClose();
	private:  
		int socketDescriptor;  
};  

TCPSocket::TCPSocket() : socketDescriptor(-1) {}  

TCPSocket::~TCPSocket() {  
	if (socketDescriptor != -1) {  
		close(socketDescriptor);  
	}  
}  

bool TCPSocket::initialize() {  
	socketDescriptor = socket(AF_INET, SOCK_STREAM, 0);  
	if (socketDescriptor == -1) {  
		std::cerr << "ntrip failed to create socket" << std::endl;
		return false;  
	}  
	return true;  
}  

bool TCPSocket::connectToServer(const char* ipAddress, int port) {  
	if (socketDescriptor == -1) {  
		std::cerr << "ntrip socket is not initialized" << std::endl;
		return false;  
	}  

	sockaddr_in serverAddr;  
	memset(&serverAddr, 0, sizeof(serverAddr));  
	serverAddr.sin_family = AF_INET;  
	serverAddr.sin_port = htons(port);  
	serverAddr.sin_addr.s_addr = inet_addr(ipAddress);  

	if (connect(socketDescriptor, (sockaddr*)&serverAddr, sizeof(serverAddr)) == -1) {  
		std::cerr << "ntrip failed to connect to server" << std::endl;
		return false;  
	}  
	return true;  
}  

bool TCPSocket::sendData(const char* data, size_t length) {  
	if (socketDescriptor == -1) {  
		std::cerr << "ntrips ocket is not initialized or connected" << std::endl;
		return false;  
	}  

	ssize_t bytesSent = send(socketDescriptor, data, length, 0);  
	if (bytesSent == -1) {  
		std::cerr << "ntrip failed to send data" << std::endl;
		return false;  
	}  
	return true;  
}  

bool TCPSocket::receiveData(char* buffer, size_t bufferSize, size_t& receivedLength) {  
	if (socketDescriptor == -1) {  
		std::cerr << "ntrip socket is not initialized or connected" << std::endl;
		return false;  
	}  

	ssize_t bytesRead = recv(socketDescriptor, buffer, bufferSize - 1, 0); // -1 to leave room for null terminator  
	if (bytesRead == -1) {  
		std::cerr << "ntrip failed to receive data" << std::endl;
		return false;  
	} else if (bytesRead == 0) {  
		// Connection closed by peer  
		std::cout << "ntrip connection closed by server" << std::endl;
		return false;  
	}  

	buffer[bytesRead] = '\0'; // Null-terminate the received string  
	receivedLength = bytesRead;  
	return true;  
} 


bool TCPSocket::sdClose() {  
	if (socketDescriptor != -1) {  
		close(socketDescriptor);  
	}  
}  

#endif
